#include "hw_control.h"
#include "uart.h"

volatile uint16_t RX_ail=1000,RX_ele=1000,RX_thr=1000,RX_rud=1000;
volatile uint8_t RX_aux = 0;
volatile uint16_t Uart_CTRL_timer=0;
void UART_init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2 | RCC_APB1Periph_USART3, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB , ENABLE);
	USART_InitTypeDef USART_InitStructure;

	// UART 1 TX
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 ;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	// UART 1 RX
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	USART_InitStructure.USART_BaudRate = 115200;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b ;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;

	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
	USART_Init(USART1, &USART_InitStructure);
    USART_Cmd(USART1, ENABLE);
}

volatile uint8_t DEBUG_RX_buf[256];

void uart1_packet_recv(u8 data)
{
	static unsigned char index =0;
	static u8 packet_size =0,checksum=0;;
	// recv data all buffer save
	if((index==0)&&(data == 'F'))
	{
		DEBUG_RX_buf[index]= data;
	 	index=1;

		checksum=0;
	}
	else if((index==1)&&(data =='C'))
	{
		DEBUG_RX_buf[index]= data;
		index=2;

		checksum=0;
	}
	else if(index == 2)
	{
		DEBUG_RX_buf[index]= data;
		index =3;

		checksum^=data;
		switch (data)
		{
			case FLIGHT_INFO :
				break;
			case OSD_INFO:
				break;
			case GPS_CONTROL_INFO :
				break;
			case VERSION_INFO:
				break;
			case REQUEST:
				break;
			case WAYPOINT:
				break;
			case PC_CONTROL:
				packet_size = sizeof(PC_control)+2;
				Uart1_putchar(packet_size);
				break;

			default : 	index = 0; packet_size =0;
		}
	}
	else if(index <packet_size)
	{
		DEBUG_RX_buf[index]= data;
		index++;

		checksum^=data;
	}
	else if(index==packet_size)
	{
		DEBUG_RX_buf[index]= data;

		checksum^=data;

		Uart1_putchar(checksum);
		if(checksum==0)
		{
			switch(DEBUG_RX_buf[2])
			{

				case FLIGHT_INFO :
					break;

				case GPS_CONTROL_INFO:
					break;

				case OSD_INFO:

					break;

				case OPTION1_INFO :
					break;

				case VERSION_INFO :
					break;

				case REQUEST:
					asm("nop");
					break;

				case WAYPOINT:
					break;

				case PC_CONTROL :
					Uart1_putchar('o');
					Uart1_putchar('k');
					memcpy((uint8_t *)(&PC_control.label), (uint8_t *)&DEBUG_RX_buf[2],sizeof(PC_control));
					Uart_CTRL_timer = 5000;
					RX_ail = PC_control.roll;
					RX_ele = PC_control.pitch;
					RX_thr = PC_control.thr;
					RX_rud = PC_control.yaw;
					break;
				default : break;
			}
		}
		packet_size=0;
		index =0;
		memset((uint8_t *)&DEBUG_RX_buf,0,sizeof(DEBUG_RX_buf));
	}
}


void Uart1_putchar(u8 c)
{
	USART_SendData(USART1,(uint8_t) c);
	while(USART_GetFlagStatus(USART1,USART_FLAG_TXE) == RESET);
	asm("NOP");asm("NOP");
}

//UART1 Q Buffer
#define USART1_BUFFER_SIZE  1024
__IO u8 U1_rx_buffer[USART1_BUFFER_SIZE];
uint32_t U1_rx_point_head=0;
uint32_t U1_rx_point_tail=0;

uint8_t uart1_is_empty(void)
{
	if(U1_rx_point_head == U1_rx_point_tail) {
		return 1;
	}
	return 0;
}

void u1_increase_point_value(u32 *p)
{
	(*p)++;
	if(USART1_BUFFER_SIZE == (*p))
	{
		(*p) = 0;
	}
}

u8 uart1_dequeue(void)
{
	u8 rev = U1_rx_buffer[U1_rx_point_tail];
	u1_increase_point_value(&U1_rx_point_tail);
	return rev;
}

void Uart1_enqueue(u8 data)
{
	U1_rx_buffer[U1_rx_point_head] = data;
	u1_increase_point_value(&U1_rx_point_head);
}


void USART1_IRQHandler(void)
{
	uint8_t cc;
	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) {
		USART_ClearITPendingBit(USART1, USART_IT_RXNE);
		cc = USART_ReceiveData(USART1);
		Uart1_enqueue(cc);
		uart1_packet_recv(cc);
	}
}

